#pragma once

#include <memory>
#include <string>
#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include "bt_manager/blackboard.h"
#include "bt_manager/auto_conditions.h"
#include "bt_manager/auto_actions.h"
#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>

namespace Sentry_BT
{
class BTManager
{
public:
  BTManager();
  ~BTManager() = default;
  
  bool initialize(const std::string& xml_file_path, std::shared_ptr<BT::Blackboard> blackboard);
  
  void execute();
  
  BT::NodeStatus getStatus() const;
  
private:
  BT::BehaviorTreeFactory factory_;
  BT::Tree tree_;
};
}  // namespace Sentry_BT

// ros_interface.h
#pragma once

// ROS1 Libraries
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>

// ROS1 Message Libraries
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>

// Custom Messages
#include "robot_msgs/EventStatus.h"

// Project Headers
#include "bt_manager/blackboard.h"
#include "bt_manager/tf_utils.h"
#include "nav_zone.h"

namespace Sentry_BT
{
  class RosInterface
  {
  private:
    ros::Subscriber event_sub;
    ros::Publisher outpost_pub;
    ros::Subscriber odom_sub;
    ros::Timer timer_;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> nav_client_;
    std::shared_ptr<Blackboard> blackboard_;

    int nav_status_;
    geometry_msgs::Pose current_pose_;
    void eventCallback(const robot_msgs::EventStatus::ConstPtr& msg);

  public:
    RosInterface(std::shared_ptr<Blackboard>& blackboard_ptr, ros::NodeHandle& nh);
    ~RosInterface() = default;

    bool publishNavigationGoal(const Sentry_BT::Point2D& goal);
    bool transformPose(const geometry_msgs::Pose& input_pose, geometry_msgs::Pose& output_pose);
  };
}  // namespace Sentry_BT